#include "Rp2SKController.h"

using namespace Rp2;

RP2_IMPLEMENT_RTTI(Rp2, SKController, Controller);

//---------------------------------------------------------------------------------------------------
SKController::SKController()
{
    m_iTLastIndex = 0;
    m_iRLastIndex = 0;
    m_iSLastIndex = 0;
}
//---------------------------------------------------------------------------------------------------
SKController::~SKController()
{
}
//---------------------------------------------------------------------------------------------------
void SKController::GetKeyInfo(float fCtrlTime, int iQuantity, float* afTime,
									int& riLastIndex, float& rfTime, int& ri0, int& ri1)
{
	if (fCtrlTime <= afTime[0])
	{
		rfTime = 0.0f;
		riLastIndex = 0;
		ri0 = 0;
		ri1 = 0;
		return;
	}

	if (fCtrlTime >= afTime[iQuantity - 1])
	{
		rfTime = 0.0f;
		riLastIndex = iQuantity - 1;
		ri0 = riLastIndex;
		ri1 = riLastIndex;
		return;
	}

	int iNextIndex;
	if (fCtrlTime > afTime[riLastIndex])
	{
		iNextIndex = riLastIndex + 1;
		while(fCtrlTime >= afTime[iNextIndex])
		{
			riLastIndex = iNextIndex;
			iNextIndex++;
		}

		ri0 = riLastIndex;
		ri1 = iNextIndex;
		rfTime = (fCtrlTime - afTime[ri0]) / (afTime[ri1] - afTime[ri0]);
	}
	else if (fCtrlTime < afTime[riLastIndex])
	{
		iNextIndex = riLastIndex - 1;
		while(fCtrlTime <= afTime[iNextIndex])
		{
			riLastIndex = iNextIndex;
			iNextIndex--;
		}

        ri0 = iNextIndex;
        ri1 = riLastIndex;
        rfTime = (fCtrlTime - afTime[ri0]) / (afTime[ri1] - afTime[ri0]);
	}
	else
	{
		rfTime = 0.0f;
        ri0 = riLastIndex;
        ri1 = riLastIndex;
	}
}
//---------------------------------------------------------------------------------------------------
void SKController::SetTranslate(int iBoneIndex, int iFrameIndex, Vector3f& rkData)
{
	Vector3f* akTData = TranslationData->GetData();
	int iOffset = iBoneIndex*FrameQuantity[0] + iFrameIndex;
	akTData[iOffset] = rkData;
}
//---------------------------------------------------------------------------------------------------
void SKController::SetRotate(int iBoneIndex, int iFrameIndex, Quaternionf& rkData)
{
	Quaternionf* akRData = RotationData->GetData();
	int iOffset = iBoneIndex*FrameQuantity[0] + iFrameIndex;
	akRData[iOffset] = rkData;
}
//---------------------------------------------------------------------------------------------------
void SKController::SetScale(int iBoneIndex, int iFrameIndex, float fData)
{
	float* afSData = ScaleData->GetData();
	int iOffset = iBoneIndex*FrameQuantity[0] + iFrameIndex;
	afSData[iOffset] = fData;
}
//---------------------------------------------------------------------------------------------------
Vector3f SKController::GetTranslate(int iBoneIndex, float fNormTime, int i0, int i1)
{
	Vector3f* akTData = TranslationData->GetData();
	int iOffset = iBoneIndex*FrameQuantity[0];
	return akTData[i0+iOffset] + fNormTime*(akTData[i1+iOffset] - akTData[i0+iOffset]);
}
//---------------------------------------------------------------------------------------------------
Matrix4f SKController::GetRotate(int iBoneIndex, float fNormTime, int i0, int i1)
{
	Quaternionf* akRData = RotationData->GetData();
	int iOffset = iBoneIndex*FrameQuantity[0];
	Quaternionf kQ;
	kQ.Slerp(fNormTime, akRData[i0+iOffset], akRData[i1+iOffset]);
	
	Vector3f Axis;
	float Angle;
	kQ.ToAxisAngle(Axis, Angle);
	return Matrix4f(Axis,Angle);
}
//---------------------------------------------------------------------------------------------------
float SKController::GetScale (int iBoneIndex, float fNormTime, int i0, int i1)
{
    float* afSData = ScaleData->GetData();
	int iOffset = iBoneIndex*FrameQuantity[0];
    return afSData[i0+iOffset] + fNormTime*(afSData[i1+iOffset] - afSData[i0+iOffset]);
}
//---------------------------------------------------------------------------------------------------
bool SKController::Update(double dAppTime)
{
	if (!Controller::Update(dAppTime))
	{
		assert(false);
		return false;
	}

    SkinningEffect* pkSkin = StaticCast<SkinningEffect>(m_pkObject);
    float fCtrlTime = (float)GetControlTime(dAppTime);
    float fNormTime = 0.0f;
    int i0 = 0, i1 = 0;

    GetKeyInfo(fCtrlTime,Times->GetQuantity(),Times->GetData(),m_iTLastIndex,fNormTime,i0,i1);
	
	std::vector<Bone*>& akBones = pkSkin->GetBones();

	for (int i = 0; i < (int)akBones.size(); i++)
	{
		Bone* pkBone = akBones[i];
		if (!pkBone->HasParent)
		{
			// update from root bone
			UpdateLocalTransform(pkBone, Vector4f::ZERO, Matrix4f::IDENTITY, Matrix4f::IDENTITY,
				Matrix4f::IDENTITY,	fNormTime, i0, i1, true);
		}
	}
    return true;
}
//---------------------------------------------------------------------------------------------------
Matrix4f SKController::GetTransform(int i, float fNormTime, int i0, int i1)
{
	//GetTranslate(i,fNormTime,i0,i1);	
	Matrix4f kM = GetRotate(i,fNormTime,i0,i1);
	float fScale = GetScale(i,fNormTime,i0,i1);
	if (fabs(fScale - 1.0f) > 0.001f)
	{
		kM[0] *= fScale;
		kM[5] *= fScale;
		kM[10] *= fScale;
	}

	return kM;
}
//---------------------------------------------------------------------------------------------------
void SKController::UpdateLocalTransform(Bone* pkBone, const Vector4f& rkTw_Parent, const Matrix4f& rkMOw_Parent,
	const Matrix4f& rkMRot, const Matrix4f& rkG, float fNormTime, int i0, int i1, bool bRootBone)
{
	// translate
	if (bRootBone)
	{
		pkBone->WorldTranslate = Vector4f(pkBone->GetRelTranslate(), 1.0f);
	}
	else
	{
		Vector4f kTr = Vector4f(pkBone->GetRelTranslate(), 1.0f);
		pkBone->WorldTranslate = rkTw_Parent + kTr*rkMOw_Parent;
		pkBone->WorldTranslate.W() = 1.0f;
	}

	// orientation
	Vector3f kA;
	float fAngle;
	pkBone->GetRelOrientation().ToAxisAngle(kA, fAngle);
	Matrix4f kMOr = Matrix4f(kA, fAngle);
	if (bRootBone)
	{
		pkBone->WorldOrientation = kMOr;
	}
	else
	{
		pkBone->WorldOrientation = kMOr * rkMOw_Parent;		
	}

	// rotation
	Matrix4f MRot = GetTransform(pkBone->GetBoneID(), fNormTime, i0, i1);
	Matrix4f G = pkBone->WorldOrientation.Transpose() * MRot * pkBone->WorldOrientation;
	// update children translate  
	UpdateRelTransform(pkBone, MRot, pkBone->WorldOrientation);

	// calculate matrix
	Vector4f TMG = pkBone->WorldTranslate - pkBone->WorldTranslate * G;
	G[12] = TMG.X();
	G[13] = TMG.Y();
	G[14] = TMG.Z();
	if (!bRootBone)
	{
		G = rkG * G;
	}

    SkinningEffect* pkSkin = StaticCast<SkinningEffect>(m_pkObject);	
	float* afMatrix = new float[12];
	afMatrix[0] = G[0];
	afMatrix[1] = G[4];
	afMatrix[2] = G[8];
	afMatrix[3] = G[12];
	afMatrix[4] = G[1];
	afMatrix[5] = G[5];
	afMatrix[6] = G[9];
	afMatrix[7] = G[13];
	afMatrix[8] = G[2];
	afMatrix[9] = G[6];
	afMatrix[10] = G[10];
	afMatrix[11] = G[14];
	pkSkin->SetMatrix(pkBone->GetBoneID(), afMatrix);
	delete[] afMatrix;

	// restore relative transform
	if (!bRootBone)
	{
		pkBone->RestoreTransform();
	}

	for (int i = 0; i < (int)pkBone->GetQuantity(); i++)
	{
		Bone* pkChild = (Bone*)(Spatial*)pkBone->GetChild(i);
		UpdateLocalTransform(pkChild, pkBone->WorldTranslate, pkBone->WorldOrientation, MRot,
			G, fNormTime, i0, i1, false);
	}
}
//---------------------------------------------------------------------------------------------------
void SKController::UpdateRelTransform(Bone* pkBone,const  Matrix4f& rkMRot,const Matrix4f& rkOrient)
{
	for (int i = 0; i < (int)pkBone->GetQuantity(); i++)
	{
		Bone* pkChild = (Bone*)(Spatial*)pkBone->GetChild(i);
		pkChild->SaveTransform(pkChild->RelTranslate, pkChild->RelOrientation);
		//update relative translate
		Vector4f kTr = Vector4f(pkChild->RelTranslate,1.0f);
		kTr = kTr * rkMRot;
		pkChild->RelTranslate.X() = kTr.X();
		pkChild->RelTranslate.Y() = kTr.Y();
		pkChild->RelTranslate.Z() = kTr.Z();

		// update relative orientation
		Vector3f kAxis;
		float fAngle;
		pkChild->RelOrientation.ToAxisAngle(kAxis, fAngle);
		Matrix4f kMOr(kAxis, fAngle);
		kMOr = kMOr * rkMRot;

		Matrix3f kMOr3;
		kMOr3[0] = kMOr[0];
		kMOr3[1] = kMOr[1];
		kMOr3[2] = kMOr[2];
		kMOr3[3] = kMOr[4];
		kMOr3[4] = kMOr[5];
		kMOr3[5] = kMOr[6];
		kMOr3[6] = kMOr[8];
		kMOr3[7] = kMOr[9];
		kMOr3[8] = kMOr[10];
		pkChild->RelOrientation.FromRotationMatrix(kMOr3);
	}
}
//---------------------------------------------------------------------------------------------------
// name and unique id
//---------------------------------------------------------------------------------------------------
Object* SKController::GetObjectByName (const std::string& rkName)
{
    Object* pkFound = Controller::GetObjectByName(rkName);
    if (pkFound)
    {
        return pkFound;
    }

    if (Times)
    {
        pkFound = Times->GetObjectByName(rkName);
        if (pkFound)
        {
            return pkFound;
        }
    }

	if (TranslationData)
	{
		pkFound = TranslationData->GetObjectByName(rkName);
		if (pkFound)
		{
			return pkFound;
		}
	}


    if (RotationData)
    {
        pkFound = RotationData->GetObjectByName(rkName);
        if (pkFound)
        {
            return pkFound;
        }
    }

    if (ScaleData)
    {
        pkFound = ScaleData->GetObjectByName(rkName);
        if (pkFound)
        {
            return pkFound;
        }
    }

    return 0;
}
//---------------------------------------------------------------------------------------------------
void SKController::GetAllObjectsByName (const std::string& rkName,
    std::vector<Object*>& rkObjects)
{
    Controller::GetAllObjectsByName(rkName,rkObjects);

    if (Times)
    {
        Times->GetAllObjectsByName(rkName,rkObjects);
    }

    if (TranslationData)
    {
        TranslationData->GetAllObjectsByName(rkName,rkObjects);
    }

    if (RotationData)
    {
        RotationData->GetAllObjectsByName(rkName,rkObjects);
    }

    if (ScaleData)
    {
        ScaleData->GetAllObjectsByName(rkName,rkObjects);
    }
}
//---------------------------------------------------------------------------------------------------
Object* SKController::GetObjectByID (unsigned int uiID)
{
    Object* pkFound = Controller::GetObjectByID(uiID);
    if (pkFound)
    {
        return pkFound;
    }

    if (Times)
    {
        pkFound = Times->GetObjectByID(uiID);
        if (pkFound)
        {
            return pkFound;
        }
    }

    if (TranslationData)
    {
        pkFound = TranslationData->GetObjectByID(uiID);
        if (pkFound)
        {
            return pkFound;
        }
    }

    if (RotationData)
    {
        pkFound = RotationData->GetObjectByID(uiID);
        if (pkFound)
        {
            return pkFound;
        }
    }

    if (ScaleData)
    {
        pkFound = ScaleData->GetObjectByID(uiID);
        if (pkFound)
        {
            return pkFound;
        }
    }

    return 0;
}
//---------------------------------------------------------------------------------------------------

//---------------------------------------------------------------------------------------------------